#!/usr/bin/python
# -*- coding: utf-8 -*-

import rospy
from tf.msg import tfMessage
from velodyne_msgs.msg import VelodyneScan, VelodyneScanUnified
from sensor_msgs.msg import Imu
import std_msgs

def main():
  rospy.init_node('convert_msgs')
  publisher = rospy.Publisher('/velodyne_packets', VelodyneScan, queue_size=1)
  pub_imu= rospy.Publisher('/imu', Imu, queue_size=1)
  

  def callback(msg):
    scan = VelodyneScan()
    scan.header = msg.header
    scan.packets = msg.packets
    publisher.publish(scan)
    print msg.header

  def imu_callback(msg):
    h = std_msgs.msg.Header()
    h.stamp = rospy.Time.now() 
    msg.header = h
    pub_imu.publish(msg)
    

  rospy.Subscriber('/sensor/velodyne16/left/VelodyneScan', VelodyneScanUnified, callback)
  rospy.Subscriber('/sensor/novatel/Imu', Imu, imu_callback)
  rospy.spin()


if __name__ == '__main__':
  main()